#!/usr/bin/env python3
"""
简化版无人机导航系统启动文件
仅启动核心导航节点，适合生产环境
"""

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    """生成简化版launch描述"""
    
    # 获取包路径
    pkg_drone_navigation = get_package_share_directory('drone_navigation')
    
    # 参数文件路径
    params_file = os.path.join(pkg_drone_navigation, 'config', 'navigation_params.yaml')
    
    # 声明launch参数
    declare_params_file = DeclareLaunchArgument(
        'params_file',
        default_value=params_file,
        description='导航系统参数文件路径'
    )
    
    declare_enable_serial = DeclareLaunchArgument(
        'enable_serial',
        default_value='false',
        description='是否启用串口通信'
    )
    
    # 获取launch配置
    params_file_config = LaunchConfiguration('params_file')
    enable_serial = LaunchConfiguration('enable_serial')
    
    # 核心导航节点
    nodes = [
        # 导航管理器
        Node(
            package='drone_navigation',
            executable='navigation_manager',
            name='navigation_manager',
            output='screen',
            parameters=[params_file_config],
            remappings=[('/current_pose', '/Odometry')]
        ),
        
        # 障碍物检测器
        Node(
            package='drone_navigation',
            executable='obstacle_detector',
            name='obstacle_detector',
            output='screen',
            parameters=[params_file_config],
            remappings=[('/current_pose', '/Odometry')]
        ),
        
        # 路径规划器
        Node(
            package='drone_navigation',
            executable='drone_navigator',
            name='drone_navigator',
            output='screen',
            parameters=[params_file_config],
            remappings=[('/current_pose', '/Odometry')]
        ),
        
        # 速度控制器
        Node(
            package='drone_navigation',
            executable='velocity_controller',
            name='velocity_controller',
            output='screen',
            parameters=[
                params_file_config,
                {'enable_serial': enable_serial}
            ]
        ),
        
        # TF变换：base_link到livox_frame
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='base_to_livox_tf',
            arguments=['0', '0', '0.1', '0', '0', '0', '1', 'base_link', 'livox_frame']
        ),
        
        # TF变换：map到camera_init（point_lio坐标系）
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='map_to_odom_tf',
            arguments=['0', '0', '0', '0', '0', '0', '1', 'map', 'camera_init']
        )
    ]
    
    # 构建launch描述
    ld = LaunchDescription()
    
    # 添加参数声明
    ld.add_action(declare_params_file)
    ld.add_action(declare_enable_serial)
    
    # 添加所有节点
    for node in nodes:
        ld.add_action(node)
    
    return ld 